#!/usr/bin/env python3

import rospy
import time

import DrRobotController_six_axes as dr

def motor_control():
    # 初始化 ROS 节点
    rospy.init_node('motor_test_node', anonymous=True)

    '''''''运动到指定位置和姿态函数'''''''
    # dr.set_arm_pose(id_num=1, pl_temp=[200, 50, 100], theta_P_Y_R=[0, 0, 0], speed=10, acceleration=10)
    # #*********************************************************

    '''''''运动到指定位置函数'''''''
    # dr.set_arm_position(id_num=1, pl_temp=[200, 20, 30], speed=10, acceleration=10)
    # #*********************************************************
    #
    '''''''运动到指定姿态函数'''''''
    # dr.set_arm_P_Y_R(id_num=1, theta_P_Y_R=[0, 90, 0], speed=10, acceleration=10)
    # #*********************************************************

    '''''''运动到相对位置和姿态函数'''''''
    # dr.set_arm_relative_pose(id_num=1, pl_temp=[10, 5, 10],  theta_P_Y_R=[0, 0, 0], speed=10, acceleration=10)
    # #*********************************************************

    '''''''运动到相对位置函数'''''''
    # dr.set_arm_relative_position(id_num=1, pl_temp=[10, 0, 0], speed=10, acceleration=10)
    # #*********************************************************

    '''''''运动到相对姿态函数'''''''
    # dr.set_arm_relative_P_Y_R(id_num=1, theta_P_Y_R=[0, 10, 0], speed=10, acceleration=10)
    # #*********************************************************

    '''''''等待运动到位函数'''''''
    # while 1:
    #     dr.set_arm_pose(id_num=1, pl_temp=[220, -0, 100], theta_P_Y_R=[0, 0, 0], speed=30, acceleration=30)
    #     dr.pose_done(id_num=1)
    #     dr.set_arm_pose(id_num=1, pl_temp=[250, 152, 0], theta_P_Y_R=[0, 0, 0], speed=30, acceleration=30)
    #     dr.pose_done(id_num=1)
    #     dr.set_arm_pose(id_num=1, pl_temp=[250, -152, 100], theta_P_Y_R=[0, 90, 0], speed=30, acceleration=30)
    #     dr.pose_done(id_num=1)
    #     print(1)
    # # #****************************************************************************************************************

    '''''''机器人急停函数'''''''
    # dr.robot_estop(id_num=1)
    # #**********************************************************
    '''''''查看当前末端坐标x_y_z函数（内存中）'''''''
    # print(dr.show_pose_x_y_z(id_num=1))
    # *********************************************************

    '''''''查看当前末端姿态P_Y_R函数（内存中）'''''''
    # print(dr.show_pose_P_Y_R(id_num=1))
    # *********************************************************

    '''''''查看当前关节z坐标函数（内存中）'''''''
    # print(dr.show_joint_z_position(id_num=1, n=3))
    # *********************************************************

    '''''''查看当前关节x坐标函数（内存中）'''''''
    # print(dr.show_joint_x_position(id_num=1, n=3))
    # *********************************************************

    '''''''查看当前模型关节角度函数（回读关节电机角度计算）'''''''
    # print(dr.detect_joints(id_num=1, n=4))
    # *********************************************************

    '''''''查看当前末端坐标x_y_z函数（回读关节电机角度计算）'''''''
    # print(dr.detect_pose_x_y_z(id_num=1))
    # *********************************************************

    '''''''查看当前末端姿态P_Y_R函数（回读关节电机角度计算）'''''''
    # print(dr.detect_pose_P_Y_R(id_num=1))
    # #*********************************************************

    '''''''重力补偿'''''''
    # start = time.time()
    # t=0
    # while(t<20):
    #     dr.gravity_compensation(id_num=1, pay_load=0)
    #     t = time.time() - start
    # dr.out_of_gravity_compensation(id_num=1)

    '''''''退出重力补偿'''''''
    start = time.time()
    t=0
    while(t<10):
        dr.gravity_compensation(id_num=1, pay_load=0)
        t = time.time() - start
    dr.out_of_gravity_compensation(id_num=1)

    '''''''零力拖动'''''''
    # dr.zero_force_pull(id_num=1, pay_load=0)
    # #*********************************************************

    '''''''关节阻抗'''''''
    # dr.impedance_control_joints(id_num=1, kp=5, kd=5)
    # # *********************************************************

    '''''''轨迹示教（编程）'''''''
    # dr.tutorial_program(id_num=1, pay_load=0, F=[0, 0, 0], n=1000, t=10)
    # # *********************************************************

    '''''''轨迹示教（执行）'''''''
    # dr.tutorial_do(id_num=1, t=10)
    # # *********************************************************

    '''''''待机'''''''
    # dr.free(id_num=1)
    # # *********************************************************

    '''''''位姿示教（编程）'''''''
    # dr.add_pose(id_num=1, t=1)
    # # *********************************************************

    '''''''锁住'''''''
    # dr.lock(id_num=1)
    # # *********************************************************

    '''''''位姿示教（执行）'''''''
    # dr.do_pose(id_num=1, speed=10, acceleration=10, o_r=0, n=0)
    # # *********************************************************

    '''''''''''''''''''''轨迹规划函数'''''''''''''''''''''
    '''''''画正方形'''''''
    # def draw_rectangle(pl=[283, 0, -126.5], l=30, h=30):
    #     ''''在水平面上画正方形
    #     pl: 长方形左上角坐标（起始点），其中pl[2]代表作图平面与全局坐标系z轴的焦点的z坐标
    #     l: 宽度
    #     h: 高度
    #     '''
    #     n= 249 # 每条边分割的点数（数量越多画得越慢）
    #     l_delta = l/n
    #     h_delta = h/n
    #     pl_list = []
    #     pl_list.append(pl)
    #     l1 = pl[1]
    #     for i in range(1, n+1):
    #         pl_temp = [pl[0], pl[1]-i*l_delta, pl[2]]
    #         pl_list.append(pl_temp)
    #     # print(pl_temp)
    #     for i in range(1, n+1):
    #         pl_temp1 = [pl_temp[0]-i*h_delta, pl_temp[1], pl_temp[2]]
    #         pl_list.append(pl_temp1)
    #     # print(pl_temp1)
    #     for i in range(1, n+1):
    #         pl_temp2 = [pl_temp1[0], pl_temp1[1]+i*l_delta, pl_temp1[2]]
    #         pl_list.append(pl_temp2)
    #     # print(pl_temp2)
    #     for i in range(1, n+1):
    #         pl_temp3 = [pl_temp2[0]+i*h_delta, pl_temp2[1], pl_temp2[2]]
    #         pl_list.append(pl_temp3)
    #     # print(pl_temp3)
    #     # print(pl_list)
    #     return pl_list
    # #
    # #
    # pl_list = draw_rectangle(pl=[250, 100, 50], l=100, h=120) #
    # # print(pl_list)
    
    # # ########控制机械臂末端连续运动到多个指定位置和姿态函数(必须单独一次性使用)
    # # # ro.set_arm_poses(pls_temp=pl_list, theta_P_R_Ys_temp=[[0, 90, 0]], t=10) # 控制机械臂末端连续运动到多个指定位置和姿态函数(必须单独一次性使用)
    # dr.set_arm_poses_curve_pre(id_num=1, pls_temp=pl_list, theta_P_Y_Rs_temp=[[0, 90, 0]]) # 预设机械臂末端轨迹函数
    # time.sleep(1)
    # dr.set_arm_poses_curve_do(id_num=1, t=10) # 末端轨迹执行函数，参数为大致运行时间

    '''''''''''''''''''''读取机器人参数函数'''''''''''''''''''''
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.d_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.d_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_p'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_p_mass_center'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_p'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_6'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_6'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    #
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_6'))
    #
    # print(dr.read_param(id_num=1, property='dr.robot.product_mode'))
    # print(dr.read_param(id_num=1, property='dr.robot.can_id'))
    # print(dr.read_param(id_num=1, property='dr.robot.pose_done'))
    # print(dr.read_param(id_num=1, property='dr.robot.tip_can_id'))
    # # ******************************************************************************************

    '''''''''''''''''''''读取机器人关节电机参数函数'''''''''''''''''''''
    # print(dr.read_joint_motor_property(id_num=1, joint_num=1, property='dr.voltage'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=1, property='dr.i'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=1, property='dr.can.config.baud_rate'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=2, property='dr.can.config.enable_state_feedback'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=2, property='dr.version_date'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.can_id'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.state_feedback_rate_ms'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=5, property='dr.config.product_model'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_angle_limit'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_min'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_max'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.gear_ratio'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.stall_current_limit'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_crash_detect'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.crash_detect_sensitivity'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_encoder_circular_limit'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.position_done'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.position_precision'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.enable_speed_limit'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.angle_gain'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_gain'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_integrator_gain'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit_tolerance'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.inertia'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.input_filter_bandwidth'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.torque_constant'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit_margin'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.torque_limit'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_control_bandwidth'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.Iq_measured'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.Id_measured'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.enabled'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_lower'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_upper'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.enabled'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_lower'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_upper'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.speed'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.torque'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_min'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_max'))
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.enable_angle_limit'))
    # #*****************************************************************************************************************

    '''''''''''''''''''''修改机器人参数函数'''''''''''''''''''''
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_1'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.l_1', value=220)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_2'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.l_2', value=220)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_3'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.l_3', value=160)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.d_3'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.d_3', value=54.75)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.d_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.d_4'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.d_4', value=120.5)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.d_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_p'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.l_p', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_p'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_p_mass_center'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.l_p_mass_center', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.l_p_mass_center'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_1'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.G_1', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_2'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.G_2', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_3'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.G_3', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_4'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.G_4', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_5'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.G_5', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_p'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.G_p', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.G_p'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_1'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_max_1', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_2'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_max_2', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_3'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_max_3', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_4'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_max_4', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_5'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_max_5', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_6'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_max_6', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_max_6'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_1'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_min_1', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_2'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_min_2', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_3'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_min_3', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_4'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_min_4', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_5'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_min_5', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_6'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_min_6', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_min_6'))

    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_2'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_2', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_3'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_3', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_4'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_4', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_5'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_5', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_6'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_6', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_gear_ratio_6'))

    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_1'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_1', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_1'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_2'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_2', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_2'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_3'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_3', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_3'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_4'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_4', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_4'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_5'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_5', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_5'))
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_6'))
    # dr.write_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_6', value=200)
    # print(dr.read_param(id_num=1, property='dr.arm_six_axes.joints_torque_factor_6'))

    # print(dr.read_param(id_num=1, property='dr.robot.can_id'))
    # dr.write_param(id_num=1, property='dr.robot.can_id', value=2)
    # print(dr.read_param(id_num=2, property='dr.robot.can_id'))

    # print(dr.read_param(id_num=1, property='dr.robot.tip_can_id'))
    # dr.write_param(id_num=1, property='dr.robot.tip_can_id', value=5)
    # print(dr.read_param(id_num=1, property='dr.robot.tip_can_id'))

    # #*******************************************************************************************

    '''''''''''''''''''''修改机器人关节参数函数'''''''''''''''''''''
    # print(dr.read_joint_motor_property(id_num=1, joint_num=1, property='dr.can.config.baud_rate'))
    # dr.write_joint_motor_property(id_num=1, joint_num=1, property='dr.can.config.baud_rate', value=dr.read_joint_motor_property(id_num=1, joint_num=1, property='dr.can.config.baud_rate')+1)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=1, property='dr.can.config.baud_rate'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=2, property='dr.can.config.enable_state_feedback'))
    # dr.write_joint_motor_property(id_num=1, joint_num=2, property='dr.can.config.enable_state_feedback', value=1)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=2, property='dr.can.config.enable_state_feedback'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.state_feedback_rate_ms'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.state_feedback_rate_ms', value=1)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.state_feedback_rate_ms'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_angle_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_angle_limit', value=0)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_angle_limit'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_min'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_min', value=10)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_min'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_max'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_max', value=180)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.angle_max'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.stall_current_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.stall_current_limit', value=49.8)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.stall_current_limit'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_crash_detect'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_crash_detect', value=0)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_crash_detect'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.crash_detect_sensitivity'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.crash_detect_sensitivity', value=149.8)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.crash_detect_sensitivity'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_encoder_circular_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_encoder_circular_limit', value=1)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.config.enable_encoder_circular_limit'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.position_precision'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.position_precision', value=13.7)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.position_precision'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.enable_speed_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.enable_speed_limit', value=0)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.enable_speed_limit'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.angle_gain'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.angle_gain', value=10.68)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.angle_gain'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_gain'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_gain', value=0.42)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_gain'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_integrator_gain'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_integrator_gain', value=5.2)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_integrator_gain'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit', value=10)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit_tolerance'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit_tolerance', value=1.3)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.speed_limit_tolerance'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.inertia'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.inertia', value=0.01)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.inertia'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.input_filter_bandwidth'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.input_filter_bandwidth', value=21)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.controller.config.input_filter_bandwidth'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit', value=23.98)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit_margin'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit_margin', value=1.2)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_limit_margin'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.torque_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.torque_limit', value=90.1)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.torque_limit'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_control_bandwidth'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_control_bandwidth', value=21.2)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor.config.current_control_bandwidth'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.enabled'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.enabled', value=0)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.enabled'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_lower'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_lower', value=110)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_lower'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_upper'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_upper', value=119)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.board_temperature.config.temp_limit_upper'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.enabled'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.enabled', value=0)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.enabled'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_lower'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_lower', value=79)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_lower'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_upper'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_upper', value=99)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.motor_temperature.config.temp_limit_upper'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_min'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_min', value=-178.2)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_min'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_max'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_max', value=178.2)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.angle_max'))

    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.enable_angle_limit'))
    # dr.write_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.enable_angle_limit', value=1)
    # print(dr.read_joint_motor_property(id_num=1, joint_num=3, property='dr.output_shaft.enable_angle_limit'))
    # #****************************************************************************************************************************

    '''''''''''''''''''''保存机器人关节参数函数'''''''''''''''''''''
    # dr.motor_control_save_config(id_num=1, joint_num=5)
    # # *********************************************************

    '''''''''''''''''''''单关节电机设置零点位置函数'''''''''''''''''''''
    # dr.motor_control_set_zero_position(id_num=1, joint_num=5)
    # #******************************************************************************

    '''''''''''''''''''''单关节电机绝对角度控制函数'''''''''''''''''''''
    # dr.motor_control_set_angle(id_num=1, joint_num=5, angle=10, speed=2, param=10, mode=1)
    # #******************************************************************************************

    '''''''''''''''''''''单关节电机相对角度控制函数'''''''''''''''''''''
    # dr.motor_control_step_angle(id_num=1, joint_num=2, angle=-10, speed=10, param=10, mode=1)
    # #*******************************************************************************************

    '''''''''''''''''''''单关节电机力位混合控制函数'''''''''''''''''''''
    # dr.motor_control_set_angle_adaptive(id_num=1, joint_num=5, angle=-30, speed=1, torque=10)
    # #*******************************************************************************************

    '''''''''''''''''''''单关节电机阻抗控制函数'''''''''''''''''''''
    # dr.motor_control_impedance_control(id_num=1, joint_num=5, angle=0, speed=1.2, tff=0.2, kp=1, kd=1)
    # #****************************************************************************************************

    '''''''''''''''''''''单关节电机运动助力函数'''''''''''''''''''''
    # dr.motor_control_motion_aid(id_num=1, joint_num=5, angle=-90, speed=3.3, angle_err=0.1, speed_err=0.2, torque=10)
    # #*******************************************************************************************************************

    '''''''''''''''''''''单关节电机转速控制函数'''''''''''''''''''''
    # dr.motor_control_set_speed(id_num=1, joint_num=5, speed=10, param=1, mode=1)
    # #******************************************************************************

    '''''''''''''''''''''等待单关节电机转动到位函数'''''''''''''''''''''
    # dr.motor_control_set_angle(id_num=1, joint_num=5, angle=10, speed=10, param=10, mode=1)
    # dr.motor_control_position_done(id_num=1, joint_num=5)
    # dr.motor_control_set_angle(id_num=1, joint_num=5, angle=0, speed=10, param=10, mode=1)

    '''''''''''''''''''''单关节电机转速控制函数 & 急停函数'''''''''''''''''''''
    # dr.motor_control_set_speed(id_num=1, joint_num=5, speed=-10, param=10, mode=1)
    # time.sleep(2)
    # dr.motor_control_estop(id_num=1, joint_num=5)
    # #*******************************************************************************

    '''''''''''''''''''''单关节电机力矩控制函数 & 急停函数'''''''''''''''''''''
    # dr.motor_control_set_torque(id_num=1, joint_num=5, torque=10, param=5, mode=1)
    # time.sleep(2)
    # dr.motor_control_estop(id_num=1, joint_num=5)
    # #******************************************************************************


    '''''''''''''''''''''单关节电机转速自适应函数'''''''''''''''''''''
    # dr.motor_control_set_speed(id_num=1, joint_num=5, speed=10, param=10, mode=1)
    # time.sleep(1)
    # dr.motor_control_set_speed_adaptive(id_num=1, joint_num=5, speed_adaptive=1)
    # time.sleep(2)
    # dr.motor_control_estop(id_num=1, joint_num=5)
    # #*******************************************************************************

    '''''''''''''''''''''单关节电机力矩自适应函数'''''''''''''''''''''
    # dr.motor_control_set_torque(id_num=1, joint_num=5, torque=-5, param=5, mode=1)
    # time.sleep(1)
    # dr.motor_control_set_torque_adaptive(id_num=1, joint_num=5, torque_adaptive=1)
    # time.sleep(1)
    # dr.motor_control_estop(id_num=1, joint_num=5)
    # #*******************************************************************************
    '''''''''''''''''''''例程结束'''''''''''''''''''''

if __name__ == '__main__':
    try:
        motor_control()
    except rospy.ROSInterruptException:
        rospy.logerr("ROS 中断异常")

